#include "servo.h"
#include "motor.h"
#include "tim2.h"
#include "tim3.h"

/**
  * 函    数：舵机设置角度
  * 参    数：Angle 要设置的舵机角度，范围：0~180
  * 返 回 值：无
  */
void Servo1_SetAngle(float Angle)
{
	PWM_SetCompare1(Angle / 180 * 2000 + 500);	//设置占空比
												//将角度线性变换，对应到舵机要求的占空比范围上
}



/**
  * 函    数：舵机设置角度
  * 参    数：Angle 要设置的舵机角度，范围：0~180
  * 返 回 值：无
  */
void Servo2_SetAngle(float Angle)
{
	PWM_SetCompare2(Angle / 180 * 2000 + 500);	//设置占空比
												//将角度线性变换，对应到舵机要求的占空比范围上
}

static u8 S1_val = 0;

void Servol_Stop()
{
    PWM_SetCompare1(0);
    MotorPose.S1 = S1_val;
}


//20000 = 20ms
// 2ms = 2000
void Servo1_Set(void)
{
    if(S1_val == 0)S1_val = 1;
    else S1_val = 0;
    PWM_SetCompare1(500);
    tim3_setCallBack(Servol_Stop, 37);
    
}

void Servo1_Reset(void)
{
    if(S1_val == 0)S1_val = 1;
    else S1_val = 0;
    PWM_SetCompare1(0);
    tim3_setCallBack(Servol_Stop, 100);
}